////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "Math.hpp"

namespace smeg {
    namespace math {
        
////////////////////////////////////////////////////////////
/// Get a valid degree angle between 0 and 360 degrees
////////////////////////////////////////////////////////////
float GetValidDegreeAngle( float _fAngleDeg ) {
    if ( _fAngleDeg < 0.0f ) {
        return _fAngleDeg + 360.0f * ( 1 + (int)( _fAngleDeg / 360.0f ) );
    } else if ( _fAngleDeg > 360.0f ) {
        return _fAngleDeg - 360.0f * ( 1 + (int)( _fAngleDeg / 360.0f ) );
    } else {
        return _fAngleDeg;
    }
}

////////////////////////////////////////////////////////////
/// Get a valid radian angle between 0 and 2PI
////////////////////////////////////////////////////////////
float GetValidRadianAngle( float _fAngleRad ) {
    if ( _fAngleRad < 0.0f ) {
        return _fAngleRad + TWO_PI * ( 1 + (int)( _fAngleRad / TWO_PI ) );
    } else if ( _fAngleRad > TWO_PI ) {
        return _fAngleRad - TWO_PI * ( 1 + (int)( _fAngleRad / TWO_PI ) );
    } else {
        return _fAngleRad;
    }
}

// Float is zero by ZERO_EPSILON
bool IsZeroByEpsilon( float _f ) {
    return fabs( _f ) <= ZERO_EPSILON;
}

// Float is equal by ZERO_EPSILON
bool IsEqualByEpsilon( float _f1, float _f2 ) {
    return fabs( _f1 - _f2 ) <= ZERO_EPSILON;
}

// Transform a point from world space to an agent's local space
sf::Vector2f PointToLocalSpace( const sf::Vector2f& _Point, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide,
                                const sf::Vector2f& _AgentPosition ) {
    float fX = -Dot( _AgentPosition, _AgentHeading ),
        fY = -Dot( _AgentPosition, _AgentSide );

    sf::Transform matTrans(  _AgentHeading.x, _AgentHeading.y, fX,
                        _AgentSide.x, _AgentSide.y, fY,
                        0.0f, 0.0f, 1.0f);

    return matTrans.transformPoint( _Point );
}

// Transform a vector from world space to an agent's local space
sf::Vector2f VectorToLocalSpace( const sf::Vector2f& _Vector, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide ) {
    sf::Transform matTrans( _AgentHeading.x, _AgentHeading.y, 0.0f,
                            _AgentSide.x, _AgentSide.y, 0.0f,
                            0.0f, 0.0f, 1.0f);

    return matTrans.transformPoint( _Vector );
}

// Transform a point from an agent's local space into world space
sf::Vector2f PointToWorldSpace( const sf::Vector2f& _Point, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide,
                                const sf::Vector2f& _AgentPosition ) {
    sf::Transform matTrans( _AgentHeading.x, _AgentSide.x, _AgentPosition.x,
                            _AgentHeading.y, _AgentSide.y, _AgentPosition.y,
                            0.0f, 0.0f, 1.0f);

    return matTrans.transformPoint( _Point );
}

// Transform a vector from an agent's local space into world space
sf::Vector2f VectorToWorldSpace( const sf::Vector2f& _Vector, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide ) {
    sf::Transform matTrans( _AgentHeading.x, _AgentSide.x, 0.0f,
                            _AgentHeading.y, _AgentSide.y, 0.0f,
                            0.0f, 0.0f, 1.0f);

    return matTrans.transformPoint( _Vector );
}

    } // namespace math
} // namespace smeg